/**
 * 
 */
package br.edu.ufcg.embedded.util.algo;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Stack;

import org.jgrapht.Graph;
import org.jgrapht.Graphs;
import org.jgrapht.graph.DefaultEdge;

import br.embedded.socialapp.business.Usuario;

/**
 * @author daniel
 *
 */
public class AStarAlgorithm
{
	public static List<Usuario> search(Graph<Usuario, DefaultEdge> graph, Usuario source, Usuario target)
	{
		Map<String, AStarNode> openSet = new HashMap<String, AStarNode>();
		PriorityQueue<AStarNode> pQueue = new PriorityQueue<AStarNode>(20, new AStarNodeComparator());
		Map<String, AStarNode> closeSet = new HashMap<String, AStarNode>();
		AStarNode start = new AStarNode(source, 0, 1.0);
		openSet.put(source.getLogin(), start);
		pQueue.add(start);

		AStarNode goal = null;
		while(openSet.size() > 0){
			AStarNode x = pQueue.poll();
			openSet.remove(x.getNode().getLogin());
			if(x.getNode().getLogin().equals(target.getLogin())){
				//found
				//if(log.isDebugEnabled()){
				//System.out.println("Found target node " + x.getNode().getLogin());
				//}
				goal = x;
				break;
			}else{
				//if(log.isDebugEnabled()){
				//System.out.println("Search for node " + x.getNode().getLogin());
				//}
				closeSet.put(x.getNode().getLogin(), x);
				List<Usuario> neighbors = Graphs.neighborListOf(graph, x.getNode());
				for (Usuario neighbor : neighbors) {
					AStarNode visited = closeSet.get(neighbor.getLogin());
					if (visited == null) {
						double g = x.getG() + 1.0;
						AStarNode n = openSet.get(neighbor.getLogin());

						if (n == null) {
							//not in the open set
							n = new AStarNode(neighbor, g, 1.0);
							n.setCameFrom(x);
							openSet.put(neighbor.getLogin(), n);
							pQueue.add(n);
						} else if (g < n.getG()) {
							//Have a better route to the current node, change its parent
							n.setCameFrom(x);
							n.setG(g);
							n.setH(1.0);
						}
					}
				}
			}
		}

		//after found the target, start to construct the path 
		if(goal != null){
			Stack<Usuario> stack = new Stack<Usuario>();
			List<Usuario> list = new ArrayList<Usuario>();
			stack.push(goal.getNode());
			AStarNode parent = goal.getCameFrom();
			while(parent != null){
				stack.push(parent.getNode());
				parent = parent.getCameFrom();
			}
			//if (log.isDebugEnabled()) {
			//System.out.println("Constructing search path: ");
			//}
			while(stack.size() > 0){
				//if (log.isDebugEnabled()) {
				//System.out.println("\t" + stack.peek().getLogin());
				//}
				list.add(stack.pop());
			}
			return list;
		}

		return null;  
	}
}
